{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "%matplotlib inline\n",
    "%load_ext autoreload\n",
    "%autoreload 2\n",
    "from local_plot import *\n",
    "from utils import *\n",
    "def read(folder, nodes, dte=None):\n",
    "    paths = {}\n",
    "    paths_pgo = {}\n",
    "    t0 = None\n",
    "    for i in nodes:\n",
    "        output_folder = folder + str(i) + \"/\"\n",
    "        _paths, t0 = read_paths(output_folder, nodes, t0=t0, dte=dte)\n",
    "        _paths_pgo, t0 = read_paths(output_folder, nodes, prefix=\"pgo\", t0=t0, dte=dte)\n",
    "        paths[i] = _paths[i]\n",
    "        paths_pgo[i] = _paths_pgo[i]\n",
    "    return paths, paths_pgo"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "#Single drone\n",
    "nodes = [1]\n",
    "data_folder=\"/home/xuhao/output/vins\"\n",
    "dte = 140\n",
    "paths, t0 = read_paths(data_folder, nodes, prefix=\"d2vins\")\n",
    "paths_pgo, t0 = read_paths(data_folder, nodes, prefix=\"pgo\")\n",
    "\n",
    "plot_fused(nodes, paths, None, poses_pgo=paths_pgo, figsize=(15,10))\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "nodes = [1, 2, 3]\n",
    "output_folder = \"/home/xuhao/data/d2slam/ri_realsense_walkaround_2022_10/outputs/fuse_all-3/swarm\"\n",
    "dte = 110\n",
    "paths, paths_pgo = read(output_folder, nodes, dte=dte)\n",
    "plot_fused(nodes, paths, None, poses_pgo=paths_pgo, figsize=(10,6))\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.5 ('base')",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.5"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "08ce52785f0fedc81003ce387e097a83d6cc9494681cd746006386992005bb71"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
